#include "task_assignment.h"

TaskAssignment::TaskAssignment(AgvManager agvs_info,MissionManager missions_info,
                               ChargerManager chargers_info,StorePlaceManger storeplaces_info):nh_(){

    nh_ = ros::NodeHandle();
    nh_private_ = ros::NodeHandle("~");
    mission_sub_ = nh_.subscribe("/task_ack",20,&TaskAssignment::taskAckCallback,this);
    electricity_th_ = nh_private_.param("electricity_th",0.3);
    task_pub_ = nh_.advertise<taskAssignment::task>("/task",10,true);

    agvs_info_ = agvs_info;
    missions_info_ = missions_info;
    chargers_info_ = chargers_info;
    storeplaces_info_ = storeplaces_info;

    usleep(500000);
    working_th_ = boost::thread(boost::bind(&TaskAssignment::new_mission_exec,this));
}

void TaskAssignment::mission_update(Mission mission){
    missions_info_.AddMission(mission);
}

void TaskAssignment::taskAckCallback(const taskAssignment::task_ack msg){
    if(msg.ack_status == 1){//接收到命令完成响应
        multimap<int,Mission>::iterator iter;
        for(iter = missions_info_.missions_.begin();iter!=missions_info_.missions_.end();iter++){
            if(iter->second.GetMissionInfo().mission_id_ == msg.mission_id){
                //切换子任务状态(若完成则删除mission)
                int wait_task_num = iter->second.GetCurrentTask();
                if(wait_task_num!=-1){
                    iter->second.SetMissionStatue(MissionStatue::FINISH);
                }
                else{
                    missions_info_.missions_.erase(iter);
                    agvs_info_.agvs_.at(msg.agv_id).SetRobotStatues(AgvStatues::WORKING);
                    break;
                }
                //将下一个子任务分配给agv(导航还是自定义)
                if(iter->second.GetMissionInfo().MissionType_ == MissionType::TRANSPORT){
                    if(iter->second.tasks_[wait_task_num]->GetTaskType() == TaskType::NAVIGATION){
                        taskAssignment::task agv_task;
                        agv_task.task_type = 1;//导航任务
                        agv_task.agv_id = msg.agv_id;
                        agv_task.mission_id = msg.mission_id;
                        agv_task.pose = iter->second.tasks_[wait_task_num]->GetEndPose();
                        task_pub_.publish(agv_task);
                        ROS_INFO("order.destination.x:%f",agv_task.pose.position.x);
                        iter->second.tasks_[wait_task_num]->SetTaskStatue(MissionStatue::EXECUTE);
                    }
                    else if(iter->second.tasks_[wait_task_num]->GetTaskType() == TaskType::EXECUTEACTION){
                        taskAssignment::task agv_task;
                        agv_task.task_type = (int)iter->second.tasks_[wait_task_num]->GetActionType();//执行动作任务
                        agv_task.agv_id = msg.agv_id;
                        agv_task.mission_id = msg.mission_id;
                        agv_task.pose = iter->second.tasks_[wait_task_num]->GetEndPose();
                        task_pub_.publish(agv_task);
                        ROS_INFO("agv_task.task_type:%d",agv_task.task_type);
                        iter->second.tasks_[wait_task_num]->SetTaskStatue(MissionStatue::EXECUTE);
                    }
                }
                else if(iter->second.GetMissionInfo().MissionType_ == MissionType::CUSTOM){}
            }
        }
    }
}

void TaskAssignment::new_mission_exec(){
    ros::Rate rate(20);
    while(ros::ok()){
        switch (main_status_) {
        case 0:
        {
            map<int,Agv>::iterator it_agv;
            //ROS_INFO("agv_size:::%d",agvs_info_.agvs_.size());
            for(it_agv=agvs_info_.agvs_.begin();it_agv!=agvs_info_.agvs_.end();it_agv++){
                if(it_agv->second.GetRobotStatues() != AgvStatues::CHARGING&&
                        it_agv->second.GetElectricity() < electricity_th_){
                    //选择一个最近的空闲充电桩,返回充电桩id
                    int charger_id = chargers_info_.SelectFreeCharger(it_agv->second.GetAgvPose());
                    ROS_INFO("charger_id:::%d",charger_id);
                    if(charger_id!=-1){
                        it_agv->second.SetRobotStatues(AgvStatues::CHARGING);
                        //生成充电task
                        taskAssignment::task agv_task;
                        agv_task.task_type = 4;//充电任务
                        agv_task.agv_id = it_agv->second.GetAgvConstInfo().RobotId_;
                        //ROS_INFO("aaaaaaaagv_id:::%d",agv_task.agv_id);
                        for(int i = 0;i<chargers_info_.Chargers_.size();i++){
                            if(chargers_info_.Chargers_[i].GetChargerId() == charger_id){
                                agv_task.pose = chargers_info_.Chargers_[i].GetKeyPose();
                                chargers_info_.Chargers_[i].SetAvailable(false);
                                break;
                            }
                        }
                        ROS_INFO("charger::agv_task.pose.x:%f",agv_task.pose.position.x);
                        task_pub_.publish(agv_task);
                        for(int i=0;i<chargers_info_.Chargers_.size();i++){
                            chargers_info_.Chargers_[i].SetAvailable(false);
                        }
                    }
                }
                else if(it_agv->second.GetRobotStatues() == AgvStatues::IDLE&&
                        it_agv->second.GetInStorePlace() == false){
                    //选择一个最近的空闲存储点,返回存储点id
                    int place_id = storeplaces_info_.SelectFreeStore(it_agv->second.GetAgvPose());
                    //生成存储点task
                    ROS_INFO("place_id:::%d",place_id);
                    if(place_id!=-1){
                        taskAssignment::task agv_task;
                        agv_task.task_type = 5;//前往空闲点任务
                        agv_task.agv_id = it_agv->second.GetAgvConstInfo().RobotId_;
                        //ROS_INFO("agv_id:::%d",agv_id);
                        for(int i = 0;i<storeplaces_info_.StorePlaces_.size();i++){
                            if(storeplaces_info_.StorePlaces_[i].GetStoreId() == place_id){
                                agv_task.pose = storeplaces_info_.StorePlaces_[i].GetKeyPose();
                                storeplaces_info_.StorePlaces_[i].SetAvailable(false);
                                break;
                            }
                        }
                        task_pub_.publish(agv_task);
                        //ROS_INFO("store::order.destination.x:%f",order.destination.position.x);
                    }
                }
            }
            main_status_ = 1;
        }
            break;
        case 1:
        {
            //ROS_INFO("mission distibute AGV");
            multimap<int,Mission>::iterator it_m;
            for(it_m=missions_info_.missions_.begin();it_m!=missions_info_.missions_.end();it_m++){
                if(it_m->second.GetMissionStatue()==MissionStatue::WAITING){
                    //根据等待的任务选择Agv
                    int agv_id = agvs_info_.AgvSelect(it_m->second.GetMissionInfo());
                    ROS_INFO("distribute AGV_ID:::%d",agv_id);
                    //生成子任务task
                    if(agv_id != -1){
                        agvs_info_.agvs_.at(agv_id).SetRobotStatues(AgvStatues::WORKING);
                        it_m->second.AutoBuildTask();  //自动生成子任务
                        int wait_task_num = it_m->second.GetCurrentTask();  //获取第一个等待的子任务
                        if(it_m->second.tasks_[wait_task_num]->GetTaskType() == TaskType::NAVIGATION){
                            taskAssignment::task agv_task;
                            agv_task.task_type = 1;//导航任务
                            agv_task.agv_id = agv_id;
                            agv_task.mission_id = it_m->second.GetMissionInfo().mission_id_;
                            agv_task.pose = it_m->second.tasks_[wait_task_num]->GetEndPose();
                            task_pub_.publish(agv_task);
                            it_m->second.tasks_[wait_task_num]->SetTaskStatue(MissionStatue::EXECUTE);
                            it_m->second.SetMissionStatue(MissionStatue::EXECUTE);
                        }
                    }
                }
            }
            //分配任务失败
            main_status_ = 0;
        }
            break;
        default:
            break;
        }
        rate.sleep();
    }
}


